function Plot3D( angle )
%PLOT3D Summary of this function goes here
%   Detailed explanation goes here
global phi_vec;
global theta_vec;
global psi_vec;
global t;
global N;

vertex_matrix = [ 2 -2 -1; 2 2 -1; -2 2 -1; -2 -2 -1;2 -2 1; 2 2 1; -2 2 1; -2 -2 1];
faces_matrix = [ 1 2 6 5; 2 3 7 6;3 4 8 7; 4 1 8 5; 1 2 3 4; 5 6 7 8];
face_CData = hsv(6);


%  Get 3 angulars: roll, pitch, yaw 
phi = angle(1);
theta = angle(2);
psi = angle(3);

% Add data
phi_vec = [phi_vec phi];
theta_vec = [theta_vec theta];
psi_vec = [psi_vec psi];
t = [t 0.01*N];

% Compute rotation matrix 
C = [cos(phi) * cos(psi), -cos(phi) * sin(psi) + sin(phi) * sin(theta) * cos(psi), sin(phi) * sin(psi) + cos(phi) * sin(theta) * cos(psi);
cos(phi) * sin(psi), cos(phi) * cos(psi) + sin(phi) * sin(theta) * sin(psi), -sin(phi) * cos(psi) + cos(phi) * sin(theta) * sin(psi);
-sin(theta), sin(phi) * cos(theta), cos(phi) * cos(theta)];
x = C' * vertex_matrix';

% Graph 3D
subplot(2,3,1);plot(t,phi_vec * 180 / pi);title('roll'); axis([0 20 -180 180]); grid on;
subplot(2,3,2);plot(t,theta_vec * 180 / pi);title('pitch');axis([0 20 -180 180]); grid on;
subplot(2,3,3);plot(t,psi_vec * 180 / pi);title('yaw');axis([0 20 -180 180]); grid on;

% refresh
subplot(2,3,4:6);
plot3(0,0,0);   % clear screen
patch('Vertices',x','Faces',faces_matrix,'FaceVertexCData',face_CData,'FaceColor','flat')
view(3);
axis square
axis equal
axis([-3 3 -3 3 -2 2]);

end

